#include <PID_v1.h>

// 定义传感器引脚
#define LEFT_SENSOR 16
#define CENTER_SENSOR 18
#define RIGHT_SENSOR 17

// 定义电机引脚
#define MOTOR_A1 5
#define MOTOR_A2 6
#define MOTOR_B1 9
#define MOTOR_B2 10

// 设置PID参数
double Setpoint = 0; // 目标值为0，表示车辆应当保持在中心位置
double Kp = 2;       // 比例系数
double Ki = 5;       // 积分系数
double Kd = 1;       // 微分系数

double Input, Output;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
int leftSensorValue, centerSensorValue, rightSensorValue;

void setup() {
  Serial.begin(9600);
  // 配置电机引脚为输出模式
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);
  // 配置传感器引脚为输入模式
  pinMode(CENTER_SENSOR, INPUT);
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);

  // 初始化PID控制器
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);

  // 输出PID参数信息
  Serial.println("PID Tuning:");
  Serial.print("Kp: ");
  Serial.println(Kp);
  Serial.print("Ki: ");
  Serial.println(Ki);
  Serial.print("Kd: ");
  Serial.println(Kd);
}

void loop() {
  // 读取传感器数据
  leftSensorValue = digitalRead(LEFT_SENSOR);
  centerSensorValue = digitalRead(CENTER_SENSOR);
  rightSensorValue = digitalRead(RIGHT_SENSOR);
  
  // 计算传感器数据的误差
  double error = 0.5 * (double)(rightSensorValue - leftSensorValue) + (double)centerSensorValue - 0.5;

  // 将误差归一化到 [-1, 1] 范围内
  Input = error / 1023.0 * 2.0;

  // 使用PID控制器计算输出
  pid.Compute();

  // 根据PID输出来控制电机转向
  int leftSpeed = constrain(150 + Output, 0, 255); // 左轮速度
  int rightSpeed = constrain(150 - Output, 0, 255); // 右轮速度
  
  if(!(digitalRead(16))&&!(digitalRead(17))&&!(digitalRead(18))){
      //全黑 直行
      analogWrite(MOTOR_A1, leftSpeed);
      digitalWrite(MOTOR_A2, LOW);
      analogWrite(MOTOR_B1, rightSpeed);
      digitalWrite(MOTOR_B2, LOW);
  }
  else if(!(digitalRead(16))){
      // 控制右轮转速增加，左转
      analogWrite(MOTOR_A1, leftSpeed);
      digitalWrite(MOTOR_A2, LOW);
      analogWrite(MOTOR_B1, 0);
      digitalWrite(MOTOR_B2, rightSpeed);
      }
  else if(!(digitalRead(17))){
      // 控制左轮转速增加，右转
      analogWrite(MOTOR_A1, 0);
      digitalWrite(MOTOR_A2, leftSpeed);
      analogWrite(MOTOR_B1, rightSpeed);
      digitalWrite(MOTOR_B2, LOW);
      }
  else if(digitalRead(16)&&digitalRead(17)&&digitalRead(18)){
    //全白 停止
      analogWrite(MOTOR_A1, 0);
      digitalWrite(MOTOR_A2, LOW);
      analogWrite(MOTOR_B1, 0);
      digitalWrite(MOTOR_B2, LOW);
      }
  else{
    //直行
      analogWrite(MOTOR_A1, leftSpeed);
      digitalWrite(MOTOR_A2, LOW);
      analogWrite(MOTOR_B1, rightSpeed);
      digitalWrite(MOTOR_B2, LOW);
      }

  // 输出调试信息
  Serial.print("Input: ");
  Serial.print(Input);
  Serial.print(" Output: ");
  Serial.println(Output);

  delay(10); // 延迟一段时间
}
